#include<iostream>
#include<fstream>
#include<iomanip>
#include "Frame.h"
#include "Kdtree_search.h"
#include "chrono"
#include "ransac.h"
#include "triangle.h"
#include "pose_estimate_3d2d.h"
#include "MapPoints.h"
#include "ceresBundle.h"
#include "pcl_viewer.h"

#include <nlohmann/json.hpp>
#include <fstream>

using namespace std;
using namespace Eigen;
using json = nlohmann::json;

vector<cv::Mat> read_images_in_folder(cv::String pattern,vector<double>& stamps);
void LoadGroundtruth(const string &poseFile, map<double,Eigen::Isometry3d>& info);
Eigen::Isometry3d poseTransform();

int main()
{

    Relocation_Slam::ORBVocabulary* mpvocabulary;
    mpvocabulary = new Relocation_Slam::ORBVocabulary();
    bool bVocLoad = mpvocabulary->loadFromBinaryFile("/home/ubuntu/xd/RelocationSlam/ORBvoc.bin");

    //读取图像和时间戳
    cv::String pattern = "/home/ubuntu/xd/cartographer_turtlebot/src/pic11/*.png";
    vector<double> stamps;
    vector<cv::Mat> imgs = read_images_in_folder(pattern,stamps);
    std::cout<<"imgs.size::"<<imgs.size()<<std::endl;
    std::cout<<"stamps.size::"<<stamps.size()<<std::endl;

    //读取位姿和时间戳
    string poseFile = "/home/ubuntu/xd/cartographer_turtlebot/idpose.txt";
    std::map<double,Eigen::Isometry3d> info;
    LoadGroundtruth(poseFile,info);

    //转换位姿坐标系
    Eigen::Isometry3d trans = poseTransform();

    //构建relo_frames
    vector<Relocation_Slam::Frame> frames;
    for(int i=0;i<imgs.size();i++){
        double t = info.lower_bound(stamps[i])->first;
        Eigen::Isometry3d pose = info.lower_bound(stamps[i])->second * trans;
        Relocation_Slam::Frame cur_f(mpvocabulary,imgs[i],pose,stamps[i]);
        frames.push_back(cur_f);
    }

    vector<Relocation_Slam::Frame*> relo_frames;
    int frame_id = 1;
    for(int i=0;i<frames.size();i=i+5){
        frames[i].SetFrameId(frame_id);
        Relocation_Slam::Frame* f = &frames[i];
        relo_frames.push_back(f);
        frame_id++;
    }

    std::cout<<"relo_frames.size::"<<relo_frames.size()<<std::endl;
    vector<cv::Point3f> m_pOriPtXYZ;
    cv::Mat K = ( cv::Mat_<double> ( 3,3 ) << 611.569, 0, 380.919, 0, 611.569,219.402, 0, 0, 1 );

    vector<Relocation_Slam::MapPoint> MapPoints;
    //每帧和其前面所有帧进行三角化
    for(int i=1;i<relo_frames.size();i++){
        for(int j=0;j<i;j++){ //j 表示 前面的帧
            vector<pair<int,int>> match_points1; ///< 存储两帧匹配的特征点索引

//            DBoW2::FeatureVector::iterator Fit = relo_frames[i]->mFeatVec.begin();
//            while(Fit!=relo_frames[i]->mFeatVec.end()){
//                std::cout<<Fit->first<<",";
//                std::cout<<Fit->second.size()<<",";
//                Fit++;
//            }

            int point_matches =  SelectByBoW(*relo_frames[i],*relo_frames[j],match_points1,1);
            vector< cv::Point3f > points_r;


//            vector<pair<int,int>> match_points1;

//            std::cout<<"matches::"<<match_points.size()<<std::endl;
            if(match_points1.size()>15){
//                RansacSelect(match_points,relo_frames[i]->mvKeyPoints,relo_frames[j]->mvKeyPoints,
//                             match_points1);

//                std::cout<<"matches::"<<match_points.size()<<",,,,"<<match_points1.size()<<std::endl;
//                if(match_points1.size()>15)
                    triangulation(relo_frames[i],relo_frames[j],K,match_points1,points_r); ///< 进行三角化
            }


            if(match_points1.size()>15){

                for(int m=0;m<match_points1.size();m++){

                    if(fabs(points_r[m].x)<15 && fabs(points_r[m].y)<15&&fabs(points_r[m].z)<15) {
                        if (relo_frames[j]->mbKpTriangle[match_points1[m].second] == false
                            &&
                            relo_frames[i]->mbKpTriangle[match_points1[m].first] == false) { ///< 若之前帧和当前帧匹配的特征点没被三角化过
                            cv::Mat p = (cv::Mat_<float>(3, 1) << points_r[m].x, points_r[m].y, points_r[m].z);
                            Relocation_Slam::MapPoint mp(p, relo_frames[i], points_r.size(),
                                                         match_points1[m].first); ///< 创建地图点并添加当前帧观测
                            mp.AddObservation(relo_frames[j], match_points1[m].second); ///< 为地图点添加之前帧观测
                            relo_frames[i]->mnMappointId[match_points1[m].first] = MapPoints.size(); ///< 为特征点绑定地图点
                            relo_frames[j]->mnMappointId[match_points1[m].second] = MapPoints.size();
                            relo_frames[i]->mbKpTriangle[match_points1[m].first] = true; ///< 设置已经三角化出地图点
                            relo_frames[j]->mbKpTriangle[match_points1[m].second] = true;
                            MapPoints.push_back(mp);
                        } else if (relo_frames[j]->mbKpTriangle[match_points1[m].second] == true
                                   && relo_frames[i]->mbKpTriangle[match_points1[m].first] ==
                                      false) { ///< 之前帧的特征点已经三角化过,需要为该地图点添加当前帧对应特征点作为观测
                            int point_id = relo_frames[j]->mnMappointId[match_points1[m].second]; ///< 之前帧的特征点对应的地图点ID
                            MapPoints[point_id].AddObservation(relo_frames[i],
                                                               match_points1[m].first); ///< 为地图点添加当前帧的观测
                            relo_frames[i]->mnMappointId[match_points1[m].first] = relo_frames[j]->mnMappointId[match_points1[m].second]; ///< 当前帧的特征点绑定地图点
                            relo_frames[i]->mbKpTriangle[match_points1[m].first] = true; ///< 当前帧的特征点已经三角化
                        } else if (relo_frames[j]->mbKpTriangle[match_points1[m].second] == false
                                   && relo_frames[i]->mbKpTriangle[match_points1[m].first] ==
                                      true) { ///< 当前帧的特征点已经三角化过,需要为该地图点添加之前帧对应特征点作为观测
                            int point_id = relo_frames[i]->mnMappointId[match_points1[m].first]; ///< 当前帧的特征点对应的地图点ID
                            MapPoints[point_id].AddObservation(relo_frames[j],
                                                               match_points1[m].second); ///< 为地图点添加之前帧的观测
                            relo_frames[j]->mnMappointId[match_points1[m].second] = relo_frames[i]->mnMappointId[match_points1[m].first]; ///< 之前帧的特征点绑定地图点
                            relo_frames[j]->mbKpTriangle[match_points1[m].second] = true; ///< 之前帧的特征点已经三角化
                        }
                    }
                }
            }
        }
    }
    std::cout<<"AllMapPoints.SIZE:::"<<MapPoints.size()<<std::endl;

//    SolveProblem(relo_frames,MapPoints);

    vector<pair<int,int>> match_points2; ///< 存储两帧匹配的特征点索引
    int point_matches =  SelectByBoW(*relo_frames[150],*relo_frames[150],match_points2,1);

    std::cout<<"match_points2::"<<match_points2.size()<<std::endl;

    cv::Mat R;
    cv::Mat t;
    poseEstimate3d2d(
            *relo_frames[150],
            *relo_frames[150],
            match_points2,
            MapPoints,
            K,
            R,
            t);


    cv::Mat Tinv = (cv::Mat_<double> (4,4) <<
            R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), t.at<double>(0,0),
            R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), t.at<double>(1,0),
            R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), t.at<double>(2,0),
            0,0,0,1);
    cout<<"Tinv:"<<endl<<Tinv<<endl;
    cv::Mat T = Tinv.inv();
    cout<<"T:"<<endl<<T<<endl;
//
    std::cout<<"img_frame.T.groundTruth:"<<endl<<relo_frames[150]->mPose.matrix()<<endl;

//    for(int i=0;i<MapPoints.size();i++){
//        int a = 0;
//        if(MapPoints[i].mObservations.size()>2){
//            std::map<Relocation_Slam::Frame*,int>::iterator it = MapPoints[i].mObservations.begin();
//            for(;it!=MapPoints[i].mObservations.end();it++){
//                Relocation_Slam::Frame* f =  it->first;
//                cv::Mat g;
//                f->image.copyTo(g);
//                cv::circle(g,cv::Point(f->mvKeyPoints[it->second].pt.x,
//                                              f->mvKeyPoints[it->second].pt.y),8,CV_RGB(255,0,0),2);
//                a++;
//                cv::imshow(to_string(a),g);
//            }
//        }
//        cv::waitKey();
//    }


    std::cout<<"ididididi::::"<<fixed << setprecision(0) << relo_frames[150]->mdPicName <<std::endl;
    pclShow(MapPoints);  ///< 显示所有地图点

    return 0;
}

vector<cv::Mat> read_images_in_folder(cv::String pattern,vector<double>& stamps)
{
    vector<cv::String> fn;

    glob(pattern, fn, false);

    vector<cv::Mat> pics;
    size_t count = fn.size(); //number of png files in images folder

    for (size_t i = 0; i < count; i++)
    {
        cv::Mat img = imread(fn[i]);
        string img_stamp(fn[i].substr(49,10));

        int stamp_b = atoi(img_stamp.c_str());
        string stamp_d = "";
        for(int j=0;j<9;j++){
            stamp_d += fn[i][j+60];
        }
        int stamp_p = atoi(stamp_d.c_str());
        double a = stamp_b + (double)stamp_p/1000000000;
        stamps.push_back(a);
        pics.push_back(img);
    }
    return pics;
}

Eigen::Isometry3d poseTransform(){

    //向base_link下0.3
    Eigen::Isometry3d only_translation = Eigen::Isometry3d::Identity();
    Eigen::Vector3d base_axisX_t = Eigen::Vector3d(0,0.03,0.64);
    Eigen::Matrix3d rotation_init = Eigen::Matrix3d::Identity();
    only_translation.pretranslate(base_axisX_t);
    only_translation.rotate(rotation_init);

    Eigen::Isometry3d x_180 = Eigen::Isometry3d::Identity();
    Eigen::AngleAxisd x_180_r(1.570795,Eigen::Vector3d::UnitY());//绕y轴逆时针旋转90
    Eigen::Matrix3d X_180_r=x_180_r.toRotationMatrix();
    Eigen::Quaterniond X_180_R(X_180_r);
    x_180.pretranslate(Eigen::Vector3d(0,0,0));
    x_180.rotate(X_180_R);

    Eigen::Isometry3d y_180 = Eigen::Isometry3d::Identity();
    Eigen::AngleAxisd y_180_r(-1.570795,Eigen::Vector3d::UnitZ());//绕Z轴顺时针旋转90
    Eigen::Matrix3d Y_180_r=y_180_r.toRotationMatrix();
    Eigen::Quaterniond Y_180_R(Y_180_r);
    y_180.pretranslate(Eigen::Vector3d(0,0,0));
    y_180.rotate(Y_180_R);

    Eigen::Isometry3d base_camera =  only_translation * x_180 * y_180;

    return base_camera;
}

void LoadGroundtruth(const string &poseFile, map<double,Eigen::Isometry3d>& info)
{
    ifstream f;
    f.open(poseFile.c_str());

    while(!f.eof())
    {
        string s;
        getline(f,s);
        if(!s.empty())
        {
            stringstream ss;
            ss << s;
            double time;
            double p_x,p_y,p_z,r_x,r_y,r_z,r_w;
            ss >> time;
            ss >> p_x;
            ss >> p_y;
            ss >> p_z;
            ss >> r_x;
            ss >> r_y;
            ss >> r_z;
            ss >> r_w;
//            time= floor(time * 100.000f + 0.5) / 100.000f;

            Eigen::Vector3d pose_t = Eigen::Vector3d(p_x,p_y,p_z);
            Quaterniond pose_q(r_w,r_x,r_y,r_z);
            Matrix3d pose_r = pose_q.toRotationMatrix();
            Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
            T.rotate(pose_r);
            T.pretranslate(pose_t);
            info.insert(make_pair(time,T));
        }
    }
}